﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Runtime.InteropServices;
using System.Windows.Forms;
using System.Threading;
using Multimedia;
using System.Timers;


namespace MCorder.motor
{
    public class Motor
    {
       
        private const double min_speed = 0.003;
        public double speed_Aim = 0.08;        //方向（1顺时针 0逆时针）
        public int DIR = 0;        //方向通道

        private int do_data = 0x00;
      
        private bool IsRun = false;//电机运行状态标记

        int i = 0, outputV1 = 1000;
        double lasterror = 0;
        public double outputV=0;
        double zerror = 0;
        public int BRDirection = 1;//1：正向，2：反向
        public int RXDirection = 1;//1:绕线，2：倒线
        private double Distance;
        double kp, ki, kd;//pid控制的三个系数

        long[] nCNTValue_old = new long[4];//上一时刻的编码器数值
        long[] nCNTValue1 = new long[4];
        uint nChannel;
        public double _velocity = 0, _velocity1 = 0, distance = 0;
        double WheelDiameter = 0.16;

        PCI1010.PCI1010_PARA_DataList DL;		// 定义公共参数结构体
        PCI1010.PCI1010_PARA_LCData LC;          // 定义直线曲线参数结构体
       
        PCI1010.PCI1010_PARA_RR0 RR0;

        IntPtr hDevice1010;
        IntPtr hDevice8932;
        int Mult = 2;
        public Motor()
        {

        }
        ~Motor()
        {
        }
        public bool isRun
        {
            get
            {
                return IsRun;
            }
        }
        //yyb
        private void OpenDevice()
        {
            hDevice1010 = PCI1010.PCI1010_CreateDevice(0);
            if (hDevice1010 == (IntPtr)(-1))
            {
                MessageBox.Show("创建PCI1010设备失败！");
                return;
            }
            int DeviceLgcID = 0;
            hDevice8932 = PCI8932.PCI8932_CreateDevice(DeviceLgcID); // 根据物理ID号创建设备对象
            if (hDevice8932 == (IntPtr)(-1))
            {
                MessageBox.Show("创建PCI8932设备失败！");
                return;
            }

            //_capturetimer = new Multimedia.Timer();
            //_capturetimer.Mode = TimerMode.Periodic;
            //_capturetimer.Period = 89;
            //_capturetimer.Resolution = 1;
            //_capturetimer.Tick += _capturetimer_Tick;



            _capturetimer_timer.Interval = 89; //这个时间单位毫秒,比如10秒，就写10000 
            _capturetimer_timer.Elapsed += new ElapsedEventHandler(_capturetimer_timer_Elapsed);

            BeginEncoder();
        }
        public void SetZeroEp()
        {
            Encoder_Reset();
            PCI1010.PCI1010_SetEP(hDevice1010, 0x01, 0);
        }

        long[] nCNTValue = new long[4];  //编码器4个通道数值，只用第0通道，电压
        //yyb
        private void BeginEncoder()
        {
            Encoder_Reset();
            // PCI8932.PCI8932_GetDeviceCNT(hDevice, ref nCNTValue[0], 0);
            _capturetimer_timer.Start();

            distance_list = new List<double>();
        }
        //yyb
        int a = 0;
        int num_nCNTValue = 0;
        public List<double> distance_list;
        private void _capturetimer_timer_Elapsed(object sender, EventArgs e)
        {
           // num_nCNTValue+=100;

            nCNTValue_old[0] = nCNTValue[0];
            nCNTValue[0] = Math.Abs(PCI1010.PCI1010_ReadEP(hDevice1010, 0x01));

            //nCNTValue[0] = num_nCNTValue;

            //PCI8932.PCI8932_GetDeviceCNT(hDevice8932, ref nCNTValue[0], 0);

            //Console.WriteLine(nCNTValue[0].ToString());

            //计算距离2147483648
            distance = ((nCNTValue[0]) / 8000.0 * WheelDiameter * 3.1415926);   //nCNTValue_old[0]
            distance_list.Add(distance);

            //Console.WriteLine/*(*/"distance..........." + distance.ToString());
            //计算速度
            _velocity = Math.Abs(nCNTValue_old[0] - nCNTValue[0]) / 8000.0 * WheelDiameter * 3.1415926 / _capturetimer_timer.Interval * 1000;
            //Console.WriteLine("_velocity" + _velocity.ToString());
            //计算平均速度
            if (a >= 6)
            {
                nCNTValue_old[1] = nCNTValue1[0];
                nCNTValue1[0] = Math.Abs(PCI1010.PCI1010_ReadEP(hDevice1010, 0x01));
               // nCNTValue[0] = num_nCNTValue;
                //PCI8932.PCI8932_GetDeviceCNT(hDevice8932, ref nCNTValue1[0], 0);
                _velocity1 = Math.Abs(nCNTValue_old[1] - nCNTValue1[0]) / 8000.0 * WheelDiameter * 3.1415926 / _capturetimer_timer.Interval * 1000 / 6;
                a = 0;
                //Console.WriteLine("_velocity1" + _velocity1.ToString());
            }
            a = 1 + a;
           
        }

        //yyb
        private void Encoder_Reset()
        {
            _velocity = 0;
            _velocity1 = 0;
            distance = 0;
            nCNTValue_old[0] = 0;
            nCNTValue[0] = 0;
            PCI1010.PCI1010_SetEP(hDevice1010, 0x01, 0);
            //PCI8932.PCI8932_SetDeviceCNT(hDevice8932, 0, 6000000, 0);
        }
        //yyb 获取速度
        public double GetVelocity()
        {
            return _velocity;
        }
        //yyb 获取平均速度
        public double GetVelocity1()
        {
            return _velocity1;
        }
        //yyb 获取距离
        public double GetDistance()
        {
            return distance;
        }
        Byte[] bDOSts, bDISts;
        private void InitArray()
        {
            bDOSts = new Byte[16];   // 对应开关量输出的各通道
            bDISts = new Byte[16];   // 对应开关量输入的各通道
            // 预置开关量输出的状态参数
            for(int i=0;i<15;i++)
            {
                bDOSts[i] = 0;
            }           
        }
        public void Init()
        {
            OpenDevice();
            InitArray();
            PCI8932.PCI8932_RetDeviceDO(hDevice8932, bDOSts);
            Thread.Sleep(1000);
        }
     
        public void stop()
        {
            setclutch(0, 0);
            distance_current = 0;
            _sdCtlTimer.Stop();
            if (p.IsAlive) p.Abort();
            InstStopRoll();
        }
        int num = 0;
        int distance_current;
        //private void startrotate1()
        //{
        //    while (true)
        //    {
        //        distance_current = num++;
        //        Thread.Sleep(30);
        //    }
        //}
        //yyb
        private void SetDA(int chan,short value)
        {
            PCI8932.PCI8932_WriteDeviceDA(hDevice8932, value, chan);
        }
        //yyb
        public void setclutch(double a, double b)//切换磁粉离合器      
        {
            if (a >= 5)
            {
                SetDA(0, 4095);
            }
            else
            {
                SetDA(0, (short)(a * 819));
            }

            if (b >= 5)
            {
                SetDA(1, 4095);
            }
            else
            {
                SetDA(1, (short)(b * 819));
            }
        }
      
        //private Multimedia.Timer _capturetimer;    //采集定时器
        System.Timers.Timer _capturetimer_timer = new System.Timers.Timer();

        Thread p;
        public void start()
        {
            p = new Thread(new ThreadStart(startrotate));
            p.IsBackground = true;
            p.Start();
        }
        private Multimedia.Timer _sdCtlTimer;    //恒线速，速度控制定时器
        private void startrotate()
        {
            if (speed_Aim <= 0.03)
            {
                Mult = 1;
                kp = 0.16;  //0.5
                ki = 0.01;   //0.03
                kd = 0.006;   //0.002
                lasterror = 0;
            }
            else if (speed_Aim > 0.03 && speed_Aim <= 0.06)//108m/hr
            {
                Mult = 2;
                kp = 0.08;  //0.5
                ki = 0.001;   //0.03
                kd = 0.0006;   //0.002
                lasterror = 0;
            }
            else if (speed_Aim > 0.06 && speed_Aim < 0.08)//216m/hr
            {
                Mult = 4;
                kp = 0.04;  //0.5
                ki = 0.03;   //0.03
                kd = 0.002;   //0.002
                lasterror = 0;
            }
            else if (speed_Aim >= 0.08)//288m/hr
            {
                Mult = 5;
                kp = 0.02;  //0.5
                ki = 0.03;   //0.03
                kd = 0.002;   //0.002
                lasterror = 0;
            }
          

            _sdCtlTimer = new Multimedia.Timer();
            _sdCtlTimer.Mode = TimerMode.Periodic;
            _sdCtlTimer.Period = 80;
            _sdCtlTimer.Resolution = 1;
            _sdCtlTimer.Tick += _sdCtlTimer_Tick;

            _sdCtlTimer.Start();//开始速度控制调节
            i = 0;
        }
        //速度调节控制
        private void _sdCtlTimer_Tick(object sender, EventArgs e)
        {
            //SetpointV = Convert.ToDouble(this.mRXSD_numericUpDown.Value);
            double speed_Cur = 0;
            speed_Cur = GetVelocity();
            //  if (GetVelocity1() < min_speed&& i > 100)
            if (speed_Cur < 0)
              {
                _sdCtlTimer.Stop();
                InstStopRoll();
                //Console.WriteLine(i.ToString());
                MessageBox.Show("速度过慢，请检查是否卡带。");
            }
            else
            {
                i++;
                double error = speed_Aim - speed_Cur;
                double derror = (error - lasterror);
                zerror = derror - zerror;
                outputV = ki * error + kp * derror + kd * (error - 2 * derror + zerror) + outputV;
                //Console.WriteLine(outputV.ToString());
                if (outputV > 2.5)
                {
                    outputV = 2.5;
                }
                if (outputV < 0.01)
                {
                    outputV = 0.01;
                }
                outputV1 = (int)(outputV / (3.1415926 * WheelDiameter) * 8000);

                //Console.WriteLine("output:" + outputV.ToString());
                //Console.WriteLine("output1:" + outputV1.ToString());
                if (outputV1 > 8000) outputV1 = 8000;

                if (DIR == 1)
                {
                    StartRoll(2, outputV1, 1, Mult);//x轴正转
                }
                else
                {
                    StartRoll(1, outputV1, 0, Mult);
                }
                //Console.WriteLine("Mult:" + Mult.ToString());

               
            }
        }
        public void InstStopRoll()
        {           
            _sdCtlTimer.Stop();

            if (DIR == 1)
            {
                StartRoll(2, 2, 1, Mult);//x轴正转
            }
            else
            {
                StartRoll(1, 2, 0, Mult);
            }

            PCI1010.PCI1010_DecStop(hDevice1010, 0xf);
            //PCI1010.PCI1010_DecStop(hDevice1010, 1);
        }
        public void StartRoll(int axis, Double speed, int direction, int multiple)
        {
          
            LC.AxisNum = axis;					// 轴号(PCI1010_XAXIS:X轴; PCI1010_YAXIS:Y轴)
            LC.LV_DV = 1;
            LC.Mode = 1;					// 模式0：CW/CCW方式，1：CP/DIR方式 
            LC.Line_Curve = 0;				// 直线曲线(0:直线加/减速; 1:S曲线加/减速)
            DL.Multiple = multiple;								// 倍数 (1~500) 
            DL.Acceleration = 2500;						// 加速度(125~1000,000)(直线加减速驱动中加速度一直不变）
            DL.AccelerationK = 2000;                    // 加速度变化率(仅S曲线驱动时有效)
            //DL.Deceleration = 4000;
            DL.StartSpeed = 2;						// 初始速度(1~8000)
            DL.DriveSpeed = (int)speed;						// 驱动速度	(1~8000)	
            LC.nPulseNum = 150000;                      // 定量输出脉冲数(0~268435455)

            LC.Direction = direction;          // 转动方向 PCI1010_PDirection: 正转  PCI1010_MDirection:反转	

            PCI1010.PCI1010_InitLVDV(				//	初始化连续,定长脉冲驱动
                                    hDevice1010,
                                    ref DL,
                                    ref LC);
            PCI1010.PCI1010_StartLVDV(				// 启动定长脉冲驱动
                                    hDevice1010,		// 设备句柄
                                    LC.AxisNum);




           // LC1010.AxisNum = 2;// axis;// PCI1010.PCI1010_XAXIS;		// 轴号(PCI1010_XAXIS:X轴,PCI1010_YAXIS:Y轴, PCI1010_ZAXIS:Z轴,PCI1010_UAXIS:U轴)
           // LC1010.LV_DV = 1;          // 驱动方式 PCI1010_DV:定长驱动 PCI1010_LV:连续驱动
           // LC1010.Mode = 0;	// 脉冲方式 PCI1010_CWCCW:CW/CCW方式,PCI1010_CPDIR:CP/DIR方式 
           // LC1010.Line_Curve =0;	// 运动方式PCI1010_LINE:直线加/减速; PCI1010_CURVE:S曲线加/减速)
           // DL1010.Multiple =  multiple;					// 倍率 (1~500) 
           // DL1010.Acceleration = 2500;			// 加速度(125~1000,000)(直线加减速驱动中加速度一直不变)
           // DL1010.AccelerationK = 2000;
           //// DL1010.Deceleration = 1000;			// 减速度(125~1000,000)(直线加减速驱动中加速度一直不变)
           // DL1010.StartSpeed = 100;			// 初始速度(1~8000)
           // DL1010.DriveSpeed =(int)speed;       //(int)(speed / (3.1415926 * Public.WheelDiameter)*2000);  // 8000;	speed/0,000251327408		// 驱动速度	(1~8000)	
           // LC1010.nPulseNum = 100000;			// 定量输出脉冲数(0~268435455)
           // LC1010.Direction = direction;// PCI1010.PCI1010_PDIRECTION;// 运动方向 PCI1010_PDIRECTION: 正转  PCI1010_MDIRECTION:反转		

           // PCI1010.PCI1010_SetLP(hDevice1010, 1, 0);   // 逻辑计数器清零
           // PCI1010.PCI1010_SetLP(hDevice1010, 2, 0);   // 实位计数器清零

           // PCI1010.PCI1010_InitLVDV(				//	初始化连续,定长脉冲驱动
           //                 hDevice1010,
           //                 ref DL1010,
           //                 ref LC1010);
           // PCI1010.PCI1010_StartLVDV(				// 启动定长脉冲驱动
           //                 hDevice1010,		// 设备句柄
           //                 LC1010.AxisNum);
        }
    }
}
